#include <iostream>
#include <string>
#include <filesystem>
#include <unistd.h>
#include "my_utils.h"
#include "json.hpp"

#define RADIAN_N1 0.01745329
#define RADIAN 57.295779513

namespace ns
{

    struct TaskDispatcher
    {
        std::string taskID;
        std::string agvID;
        std::string type;
        std::vector<float> position;
        std::string targetAGV;
    };

    void from_json(const nlohmann::json &j, ns::TaskDispatcher &t)
    {
        j.at("taskID").get_to(t.taskID);
        j.at("agvID").get_to(t.agvID);
        j.at("type").get_to(t.type);
        j.at("position").get_to(t.position);
        j.at("targetAGV").get_to(t.targetAGV);
    }
}

double Degree_to_Rad(double angle)
{
    return angle * M_PI / 180.0;
}

double Rad_to_Degree(double rad)
{
    return rad * 180 / M_PI;
}

void test_speed()
{
    SpeedUtils my_speed(1000, 200);
    my_speed.setGoal(10000);

    int walked = 0;
    int speed = 0;
    int count = 0;

    while (walked < 10000)
    {
        walked = walked + (int)speed * 0.1;
        speed = my_speed.getSpeed(walked);
        std::cout << "count: " << count << " ,speed: " << speed << " ,walked: " << walked << std::endl;
        count++;
        usleep(100 * 1000);
    }

    std::cout << "count: " << count << "speed: " << 0 << " ,walked: " << walked << std::endl;
}

void test_str2json()
{
    unsigned char buffer[] = {123, 34, 110, 97, 109, 101, 34, 58, 34, 116, 101, 115, 116, 34, 44, 34, 97, 103, 101, 34, 58, 50, 48, 125};
    size_t length = sizeof(buffer); // 或手动指定长度
    std::string str = byte2String(buffer, 0, length);
    std::cout << "str: " << str << std::endl;
    nlohmann::json data = nlohmann::json::parse(str);
    std::cout << "name: " << data["name"] << std::endl;
}

void test_json2struct()
{
    nlohmann::json data = R"({"taskID":"TASK-0001","agvID":"forklift-001","type":"pick","position":[100.5, 200.0, 90, 0, 0],"targetAGV":"unitload-001"})"_json;
    auto task = data.template get<ns::TaskDispatcher>();
    std::cout << "taskID: " << task.taskID << std::endl;
}

int main()
{
    test_json2struct();
    return 0;
}
